/**
 * @file:          Pid.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.14
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.14,00:47:47
 */

/* Include Files **************************************************************/
#include "Pid.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
uint8 Pid_Init(struct Pid *self)
{
    self->Run = Pid_Run;
    self->Clear = Pid_Clear;

    self->kp = self->param_kp;
    self->ki = self->param_ki;
    self->kd = self->param_kd;
    self->in_ref = 0;
    self->in_fdb = 0;
    self->err = 0;
    self->last_err = 0;
    self->out = 0;
    self->up = 0;
    self->ui = 0;
    self->ui_last = 0;
    self->out_presat = 0;
    self->ui_anti_windup_trim = 0;
    self->ud = 0;

    self->ud_filter.Init(&self->ud_filter);
    self->fal.Init(&self->fal);

    return TRUE;
}

float32 Pid_Run(struct Pid *self)
{
    if (self->param_en != 0)
    {
        if (self->param_bypass != 0)
        {
            self->out = self->in_ref;
        }
        else
        {
            switch (self->param_pid_mode)
            {
            case PID_MODE_KP_KI_COUPLING:
                // self->err = self->in_ref - self->in_fdb;
                // self->err = self->fal.Run(&self->fal, self->err);
                // self->up = self->err * self->kp;
                // self->ui = self->ui_last + self->err * self->up * self->param_T * self->ki - self->ui_anti_windup_trim;
                // self->ud = (self->err - self->last_err) / self->param_T * self->kd;
            // self->ud = self->ud_filter.Run(&self->ud_filter, self->ud);

                // self->ui = Limit(self->ui, -self->param_limit_ui, self->param_limit_ui);

                // self->out_presat = self->up + self->ui + self->ud;
                // self->out = Limit(self->out_presat, -self->parma_limit_out, self->parma_limit_out);

                // self->ui_anti_windup_trim = self->param_kc * (self->out_presat - self->out);

                // self->ui_last = self->ui;
                // self->last_err = self->err;
                break;
            case PID_MODE_KP_KI_INDEPENDENT:
                self->err = self->in_ref - self->in_fdb;
                self->err = self->fal.Run(&self->fal, self->err);
                self->up = self->err * self->kp;
                self->ui = self->ui_last + self->err * self->param_T * self->ki - self->ui_anti_windup_trim;
                self->ud = (self->err - self->last_err) / self->param_T * self->kd;
                self->ud = self->ud_filter.Run(&self->ud_filter, self->ud);

                self->ui = Limit(self->ui, -self->param_limit_ui, self->param_limit_ui);

                self->out_presat = self->up + self->ui + self->ud;
                self->out = Limit(self->out_presat, -self->parma_limit_out, self->parma_limit_out);

                self->ui_anti_windup_trim = self->param_kc * (self->out_presat - self->out);

                self->ui_last = self->ui;
                self->last_err = self->err;
                break;
            default:
                self->param_pid_mode = PID_MODE_KP_KI_INDEPENDENT;
                break;
            }
        }
    }
    else
    {
        self->out = 0;
    }

    return self->out;
}

void Pid_Clear(struct Pid *self)
{
    self->last_err = 0;
    self->ui_last = 0;
    self->out = 0;
}
